#include "defaults.h"
#include "trianglemesh.h"
#include "surface.h"
#include "utils.h"
#include "math/bsplines.h"
#include "math/numerics.h"

using namespace std;
	
DerivativesDim2::DerivativesDim2(int d) : D(boost::extents[d+1][d+1]), Dw(boost::extents[d+1][d+1]), d(d) {}

Surface::Surface(const vector< vector<Vec4n> >& Pw, int p, int q, const Knotvector& U, const Knotvector& V) {
	this->Pw = Pw;
	this->p = p;
	this->q = q;
	this->U = U;
	this->V = V;
	this->n = Pw.size() - 1;
	this->m = Pw[0].size() - 1;
}

Surface::Surface(const Array2V4n& pw, int p, int q, const Knotvector& U, const Knotvector& V) : p(p), q(q), U(U), V(V) {
	n = pw.shape()[0] - 1;
	m = pw.shape()[1] - 1;

	Pw.resize(n + 1);
	for(size_t i = 0; i <= n; i++) {
		Pw[i] = std::vector<Vec4n>(m + 1);
	}

	for(size_t i = 0; i <= n; i++) {
		for(size_t j = 0; j <= m; j++) {	
			Pw[i][j] = pw[i][j];
		}
	}
}
	
Surface::Surface(const Surface& surface) {
	Pw = surface.Pw;
	p = surface.p;
	q = surface.q;
	U = surface.U;
	V = surface.V;
	n = surface.n;
	m = surface.m;
}

void Surface::Save(const std::string& filename) {
	ofstream file;
	file.open(filename.c_str());
	if(!file) {
		cerr << "Could not open file \"" << filename << "\"." << endl;
	} else {
		file << "surface" << endl;
		file << p << " " << q << endl;
		file << n + 1 << " " << m + 1 << endl;
		file << endl;
		file << setprecision(32);
		for(size_t i = 0; i < Pw.size(); i++) {
			for(size_t j = 0; j < Pw[0].size(); j++) {
				file << Pw[i][j][0] << " ";
				file << Pw[i][j][1] << " ";
				file << Pw[i][j][2] << " ";
				file << Pw[i][j][3] << endl;
			}
		}
		file << endl;
		file << U.size() << endl;
		for(size_t i = 0; i < U.size(); i++)
			file << U[i] << " ";
		file << endl;
		file << V.size() << endl;
		for(size_t i = 0; i < V.size(); i++)
			file << V[i] << " ";
		file.close();
	}
}

void Surface::Save(const std::string& filename, const std::string& attribute) {
	this->Save(filename);
}

void Surface::SurfaceDerivsAlg(nfloat* param, DerivativesDim2* pd) const {
	typedef boost::multi_array<Vec4n, 2>::index index;
	boost::multi_array<Vec4n, 2>& SKL = pd->Dw;
	int d = pd->d;
	int du = d + 1;
	int dv = d + 1;
	nfloat& u = param[0];
	nfloat& v = param[1];
	
	int uspan = math::spline::FindSpan<nfloat>(n, p, u, U);
	int vspan = math::spline::FindSpan<nfloat>(m, q, v, V);
	vector< vector<nfloat> > Nu = math::spline::DersBasisFuns<nfloat>(uspan, u, p, du, U);
	vector< vector<nfloat> > Nv = math::spline::DersBasisFuns<nfloat>(vspan, v, q, dv, V);
	vector< Vec4n > temp(q + 1);
	for(index k = 0; k <= du; k++) {
		for(index s = 0; s <= q; s++) {
			temp[s] = 0.0;
			for(index r = 0; r <= p; r++)
				temp[s] = temp[s] + Nu[k][r] * Pw[uspan - p + r][vspan - q + s];	
		}
		int dd = d - k < dv ? d - k : dv;
		for(index l = 0; l <= dd; l++) {
			SKL[k][l] = 0.0;
			for(index s = 0; s <= q; s++)
				SKL[k][l] = SKL[k][l] + Nv[l][s] * temp[s];
		}
	}
}

void Surface::Derivative(nfloat* param, Derivatives* pd) const {
	DerivativesDim2& SKL = static_cast<DerivativesDim2&>(*pd);
	typedef boost::multi_array<Vec3n, 2>::index index;
	boost::multi_array<Vec4n, 2>& Awders = SKL.Dw;
	int d = SKL.d;

	SurfaceDerivsAlg(param, static_cast<DerivativesDim2*>(pd));
	for(index k = 0; k <= d; k++)
		for(index l = 0; l <= d - k; l++) {
			const Vec4n& P = Awders[k][l];
			Vec3n v = math::V3d(P[0], P[1], P[2]);
			for(index j = 1; j <= l; j++)
				v = v - Bin[l][j] * Awders[0][j][3] * SKL.D[k][l - j];
			
			for(index i = 1; i <= k; i++) {
				v = v - Bin[k][i] * Awders[i][0][3] * SKL.D[k - i][l];
				Vec3n v2;
				for(int j = 1; j <= l; j++)
					v2 = v2 + Bin[l][j] * Awders[i][j][3] * SKL.D[k - i][l - j];
				v = v - (nfloat)Bin[k][i] * v2;
			}
			SKL.D[k][l] = v * (1.0 / Awders[0][0][3]);
		}
}

Vec3n Surface::Evaluate(const boost::array<nfloat, 2>& param) const {
	const nfloat& u = param[0];
	const nfloat& v = param[1];

	std::vector<nfloat> Nu(p + 1), Nv(q + 1);

	size_t uspan = math::spline::FindSpan<nfloat>(n, p, u, U);
	size_t vspan = math::spline::FindSpan<nfloat>(m, q, v, V);
// 	math::spline::BasisFuns<nfloat>(uspan, u, p, U, Nu);
// 	math::spline::BasisFuns<nfloat>(vspan, v, q, V, Nv);
	for(size_t i = 0; i <= p; i++) Nu[i] = math::spline::BasisFun<nfloat>(u, uspan - p + i, p, U);
	for(size_t i = 0; i <= q; i++) Nv[i] = math::spline::BasisFun<nfloat>(v, vspan - q + i, q, V);


	Vec4n Sw;
	for(size_t l = 0; l <= p; l++) {
		for(size_t k = 0; k <= q; k++) {
			Sw += Nu[l] * Nv[k] * Pw[uspan - p + l][vspan - q + k];
		}
	}

	return (1.0 / Sw[3]) * math::V3n(Sw[0], Sw[1], Sw[2]);
}

Vec3n Surface::Evaluate(nfloat u, nfloat v) const {
	boost::array<nfloat, 2> param = {u, v};
	return Evaluate(param);
}

#define U_DIRECTION	0x1
#define V_DIRECTION	0x2
void Surface::RefineKnotVectSurface(int n, int p, const Knotvector& U, 
									int m, int q, const Knotvector& V,
									const vector< vector<Vec4n> >& Pw, const Knotvector& X, int r, int dir, 
									Knotvector& Ubar, Knotvector& Vbar, vector< vector<Vec4n> >& Qw) {
	if(dir == U_DIRECTION) {
		int a = math::spline::FindSpan<nfloat>(n, p, X[0], U);
		int b = math::spline::FindSpan<nfloat>(n, p, X[r], U);
		b++;

		for(int j = 0; j <= a; j++)
			Ubar[j] = U[j];
		for(int j = b + p; j <= n + p + 1; j++)
			Ubar[j + r + 1] = U[j];

		for(int row = 0; row <= m; row++) {
			for(int k = 0; k < a - p; k++)
				Qw[k][row] = Pw[k][row];
			for(int k = b - 1; k <= n; k++)
				Qw[k + r + 1][row] = Pw[k][row];
		}
		
		Vbar = V;

		int i = b + p - 1;
		int k = b + p + r;

		for(int j = r; j >= 0; j--) {
			while(X[j] <= U[i] && i > a) {
				Ubar[k] = U[i];
				for(int row = 0; row <= m; row++)
					Qw[k - p - 1][row] = Pw[i - p - 1][row];
				k--;
				i--;
			}

			for(int row = 0; row <= m; row++)
				Qw[k - p - 1][row] = Qw[k - p][row];


			for(int l = 1; l <= p; l++) {
				int ind = k - p + l;
				nfloat alfa = Ubar[k + l] - X[j];
				if(fabs(alfa) < 0.000001) {
					for(int row = 0; row <= m; row++)
						Qw[ind - 1][row] = Qw[ind][row];
				}
				else {
					alfa /= (Ubar[k + l] - U[i - p + l]);
					for(int row = 0; row <= m; row++)
						Qw[ind - 1][row] = alfa * Qw[ind - 1][row] + (1.0 - alfa) * Qw[ind][row];
				}
				
			}
			Ubar[k] = X[j];
			k--;
		}
	}
	
	if(dir == V_DIRECTION) {
		int a = math::spline::FindSpan<nfloat>(m, q, X[0], V);
		int b = math::spline::FindSpan<nfloat>(m, q, X[r], V);
		b++;

		Ubar = U;

		for(int j = 0; j <= a; j++)
			Vbar[j] = V[j];
		for(int j = b + q; j <= m + q + 1; j++)
			Vbar[j + r + 1] = V[j];

		for(int col = 0; col <= n; col++) {
			for(int k = 0; k < a - q; k++)
				Qw[col][k] = Pw[col][k];
			for(int k = b - 1; k <= m; k++)
				Qw[col][k + r + 1] = Pw[col][k];
		}

		int i = b + q - 1;
		int k = b + q + r;

		for(int j = r; j >= 0; j--) {
			while(X[j] <= V[i] && i > a) {
				Vbar[k] = V[i];
				for(int col = 0; col <= n; col++)
					Qw[col][k - q - 1] = Pw[col][i - q - 1];
				k--;
				i--;
			}

			for(int col = 0; col <= n; col++)
				Qw[col][k - q - 1] = Qw[col][k - q];

			for(int l = 1; l <= q; l++) {
				int ind = k - q + l;
				nfloat alfa = Vbar[k + l] - X[j];
				if(fabs(alfa) < 0.000001) {
					for(int col = 0; col <= n; col++)
						Qw[col][ind - 1] = Qw[col][ind];
				}
				else {
					alfa /= (Vbar[k + l] - V[i - q + l]);
					for(int col = 0; col <= n; col++)
						Qw[col][ind - 1] = alfa * Qw[col][ind - 1] + (1.0 - alfa) * Qw[col][ind];
				}
				
			}
			Vbar[k] = X[j];
			k--;
		}
	}
}

void Surface::RefineU() {
	vector<nfloat> X;

	for(size_t i = p; i < U.size() - p - 1; i++) {
		if(U[i + 1] - U[i] > 0.0001) //don't insert between multiple knots
			X.push_back(0.5 * (U[i + 1] - U[i]) + U[i]);
	}
	int r = X.size() - 1;
	
	vector< vector<Vec4n> > Qw(n + r + 2);
	for(size_t i = 0; i < Qw.size(); i++)
		Qw[i].resize(Pw[0].size());	
	
	Knotvector Ubar(U.size() + r + 1);
	Knotvector Vbar;
	
	RefineKnotVectSurface(n, p, U, m, q, V, Pw, X, r, U_DIRECTION, Ubar, Vbar, Qw);
	
	Pw = Qw;
	U = Ubar;
	V = Vbar;
	n = Pw.size() - 1;
}

void Surface::RefineV() {
	vector<nfloat> X;
	
	for(size_t i = q; i < V.size() - q - 1; i++) { 
		if(V[i + 1] - V[i] > 0.0001) //don't insert between multiple knots
			X.push_back(0.5 * (V[i + 1] - V[i]) + V[i]);
	}
	int r = X.size() - 1;
	
	vector< vector<Vec4n> > Qw(Pw.size());
	for(size_t i = 0; i < Qw.size(); i++)
		Qw[i].resize(m + r + 2);	
	
	Knotvector Ubar;
	Knotvector Vbar(V.size() + r + 1);
	RefineKnotVectSurface(n, p, U, m, q, V, Pw, X, r, V_DIRECTION, Ubar, Vbar, Qw);
	
	Pw = Qw;
	U = Ubar;
	V = Vbar;
	m = Pw[0].size() - 1;
}

void Surface::Refine() {
	RefineU();
	RefineV();
}
	
void Surface::Refine(const std::vector<Knotvector>& Xuv) {
	if(Xuv.size() != 2) {
		cerr << "error, refine: wrong input." << endl;
		return;
	}

	if(!Xuv[0].empty()) {
		const vector<nfloat>& X = Xuv[0];
		int r = X.size() - 1;

		vector< vector<Vec4n> > Qw(n + r + 2);
		for(size_t i = 0; i < Qw.size(); i++)
			Qw[i].resize(Pw[0].size());	
	
		Knotvector Ubar(U.size() + r + 1);
		Knotvector Vbar;

		RefineKnotVectSurface(n, p, U, m, q, V, Pw, X, r, U_DIRECTION, Ubar, Vbar, Qw);

		Pw = Qw;
		U = Ubar;
		V = Vbar;
		n = Pw.size() - 1;
	}

	if(!Xuv[1].empty()) {
		const vector<nfloat>& X = Xuv[1];
		int r = X.size() - 1;

		vector< vector<Vec4n> > Qw(Pw.size());
		for(size_t i = 0; i < Qw.size(); i++)
			Qw[i].resize(m + r + 2);	
		
		Knotvector Ubar;
		Knotvector Vbar(V.size() + r + 1);
		RefineKnotVectSurface(n, p, U, m, q, V, Pw, X, r, V_DIRECTION, Ubar, Vbar, Qw);

		Pw = Qw;
		U = Ubar;
		V = Vbar;
		m = Pw[0].size() - 1;
	}
}

static void boundTo(nfloat& u, nfloat um, nfloat uM) {
	if(u < um)
		u = um;
	if(u > uM)
		u = uM;
}

Matrix Surface::Jacobian(const boost::array<nfloat, 2>& param) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(1);
	Derivative(param2, &SKL);
	const Vec3n& S = (SKL.D)[0][0];
	const Vec3n& U = (SKL.D)[1][0];
	const Vec3n& V = (SKL.D)[0][1];
	
	Vec2n n = math::V2n(S[0], S[1]);
	nfloat d = S[3];
	
	Vec2n n_ = math::V2n(U[0], U[1]);
	nfloat d_ = U[2];
	Vec2n U2 = (1.0 / (d * d)) * (d * n_ - d_ * n);
	
	n_ = math::V2n(V[0], V[1]);
	d_ = V[2];
	Vec2n V2 = (1.0 / (d * d)) * (d * n_ - d_ * n);

	Matrix J(2, 2);
	J(0, 0) = U2[0]; J(0, 1) = U2[1];
	J(1, 0) = V2[0]; J(1, 1) = V2[1];
	return J;
}

void Surface::binomialCoef(Matrix& Bin) const {
	int n, k;
	// Setup the first line
	Bin(0, 0) = 1.0;
	for(k = Bin.size2() - 1; k > 0; --k)
		Bin(0, k) = 0.0;

	// Setup the other lines
	for(n = 0; n < Bin.size1() - 1; n++) {
  		Bin(n + 1, 0) = 1.0;
  		for(k = 1; k < Bin.size2(); k++)
			if(n + 1 < k)
				Bin(n, k) = 0.0 ;
			else
				Bin(n + 1, k) = Bin(n, k) + Bin(n, k - 1);
	}
}

void Surface::DegreeElevateU(int t) {
	if(t <= 0){
		return;
	}
	
	Surface S = *this;
	
	int i, j, k;
	int n = S.Pw.size() - 1;
	int p = S.GetDegreeU();
	int m = n + p + 1;
	int ph = p + t;
	int ph2 = ph / 2;
	Matrix bezalfs(p + t + 1, p + 1) ; // coefficients for degree elevating the Bezier segment
	// 	PRINTVAR(p);
	Array2V4n bpts(boost::extents[p + 1][S.Pw[0].size()]); // pth-degree Bezier control points of the current segment
	Array2V4n ebpts(boost::extents[p + t + 1][S.Pw[0].size()]) ; // (p+t)th-degree Bezier control points of the  current segment
	Array2V4n Nextbpts(boost::extents[p - 1][S.Pw[0].size()]) ; // leftmost control points of the next Bezier segment
	Vector alphas(p-1) ; // knot instertion alphas.
	
	// Compute the Binomial coefficients
	Matrix Bin(ph + 1, ph2 + 1);
	binomialCoef(Bin);
	
	// Compute Bezier degree elevation coefficients
	nfloat inv, mpi;
	bezalfs(0, 0) = bezalfs(ph, p) = 1.0;
	for(int i = 1; i <= ph2; i++) {
		inv = 1.0 / Bin(ph, i);
		mpi = std::min(p, i);
		for(int j = std::max(0, i - t); j <= mpi; j++) {
			bezalfs(i, j) = inv * Bin(p,j) * Bin(t, i - j);
		}
	}

	for(int i = ph2 + 1; i < ph ; i++) {
		mpi = std::min(p, i);
		for(int j = std::max(0, i - t); j <= mpi ; j++)
			bezalfs(i, j) = bezalfs(ph - i, p - j);
	}
	
	// Allocate more control points than necessary
	this->Pw.resize(S.Pw.size() + S.Pw.size() * t);
	for(size_t i = 0; i < Pw.size(); i++)
		this->Pw[i].resize(S.Pw[0].size());
	// 	PRINTVAR(ph);
	this->p = ph;
	this->q = S.GetDegreeV();
	this->U.resize(Pw.size() + this->p + 1);
	this->V.resize(Pw[0].size() + this->q + 1);
	
	int colJ;
	int mh = ph;
	int kind = ph + 1;
	nfloat ua = S.U[0];
	nfloat ub = 0.0;
	int r = -1; 
	int oldr;
	int a = p;
	int b = p + 1; 
	int cind = 1;
	int rbz, lbz = 1;
	int mul, save, s;
	nfloat alf;
	int first, last, kj;
	nfloat den, bet, gam, numer;

	for(size_t i = 0; i < S.Pw[0].size(); i++)
		Pw[0][i] = S.Pw[0][i];

	for(size_t i = 0; i <= ph; i++)
		U[i] = ua;
	
	// Initialize the first Bezier segment
	
	for(i = 0; i <= p; i++) {
		for(j = 0; j < S.Pw[0].size(); j++) {
			bpts[i][j] = S.Pw[i][j];
		}
	}
	
	while(b < m) { // Big loop thru knot vector
		i = b;
		if(b == 16)
			cout << b << endl;

		while(b < m && S.U[b] >= S.U[b + 1]) // for some odd reasons... == doesn't work
		  	b++;
	
		mul = b - i + 1; 
		mh += mul + t;
		ub = S.U[b];
		oldr = r;
		r = p - mul;
		if(oldr > 0)
			lbz = (oldr + 2) / 2;
		else
			lbz = 1;
		if(r > 0) 
			rbz = ph - (r + 1) / 2;
		else
			rbz = ph;

		if(r > 0) { // Insert knot to get Bezier segment
			numer = ub - ua ;
			for(k = p; k > mul; k--)
				alphas(k - mul - 1) = numer / (S.U[a + k] - ua);

			for(j = 1;j <= r; j++) {
				save = r - j;
				s = mul + j;
				for(k = p; k>= s; k--) {
					for(colJ = 0; colJ < S.Pw[0].size(); colJ++) {
					    bpts[k][colJ] = alphas(k - s) * bpts[k][colJ] + (1.0 - alphas(k - s)) * bpts[k - 1][colJ];
					}
				}
		
				if(Nextbpts.shape()[0] > 0)
					for(colJ = 0; colJ < S.Pw[0].size(); colJ++) {
						Nextbpts[save][colJ] = bpts[p][colJ];
					}
			}
		}
		
		if(b == 19)
			cout << "???1" << endl;

		for(i = lbz; i <= ph; i++) { // Degree elevate Bezier,  only the points lbz,...,ph are used
			for(colJ = 0; colJ < S.Pw[0].size(); colJ++)
				ebpts[i][colJ] = 0.0;

			mpi = std::min(p, i);
			for(j=std::max(0, i - t); j <= mpi; j++)
				for(colJ = 0; colJ < S.Pw[0].size(); colJ++)
					ebpts[i][colJ] += bezalfs(i,j) * bpts[j][colJ];
		}
		if(b == 19)
			cout << "???2" << endl;
		
		if(oldr > 1) { // Must remove knot u=c.U[a] oldr times
			first = kind - 2;
			last = kind;
			den = ub - ua;
			bet = (ub - U[kind - 1]) / den;
			for(int tr = 1; tr < oldr; tr++) { // Knot removal loop
				i = first;
				j = last;
				kj = j - kind + 1;
				while(j - i > tr) { // Loop and compute the new control points for one removal step
					if(i < cind) {
						alf = (ub - U[i]) / (ua - U[i]);
						for(colJ = 0; colJ < S.Pw[0].size(); colJ++) {
							Pw[i][colJ] = alf * Pw[i][colJ] + (1.0 - alf) * Pw[i - 1][colJ];
						}
		  			}
					if(j >= lbz) {
						if(j - tr <= kind - ph + oldr) {
							gam = (ub - U[j - tr]) / den;
							for(colJ = 0; colJ < S.Pw[0].size(); colJ++) {
								ebpts[kj][colJ] = gam * ebpts[kj][colJ] + (1.0 - gam) * ebpts[kj + 1][colJ];
							}
		    			} else {
							for(colJ = 0; colJ < S.Pw[0].size(); colJ++) {
								ebpts[kj][colJ] = bet * ebpts[kj][colJ] + (1.0 - bet) * ebpts[kj + 1][colJ];
							}
						}
					}
					++i;
					--j;
					--kj;
				}
				--first;
				++last ;
			}
		}
		if(b == 19)
			cout << "???3" << endl;
		
		if(a != p) // load the knot u=c.U[a]
			for(i = 0; i < ph - oldr; i++) {
				U[kind++] = ua; 
			}
		if(b == 19)
			cout << "???4" << endl;
		
		for(j = lbz; j <= rbz; j++) { // load control points onto nc
			for(colJ = 0; colJ < S.Pw[0].size(); colJ++)
				Pw[cind][colJ] = ebpts[j][colJ]; 
			++cind ;
		}
		if(b == 19)
			cout << "???5" << endl;
		
		if(b < m) { // Set up for next pass thru loop
			for(colJ = 0; colJ < S.Pw[0].size(); colJ++) {
				for(j = 0; j < r; j++)
					bpts[j][colJ] = Nextbpts[j][colJ];
				for(j=r;j<=p;j++)
					bpts[j][colJ] = S.Pw[b - p + j][colJ];
			}
			a = b; 
			b++;
			ua = ub;
		} else {
			for(i = 0; i <= ph; i++)
				U[kind + i] = ub;
		}
	}
	// Resize to the proper number of control points  
	//resizeKeep(mh-ph,S.P.cols(),ph,S.degV) ;
	//
	//
	if(b == 19)
		cout << "???6" << endl;

	this->p = ph;
	this->q = S.GetDegreeV();
	vector< vector<Vec4n> > Pw2(mh - ph);	
	// 	PRINTVAR(mh - ph);
	// 	PRINTVAR(S.Pw[0].size());
	for(size_t i = 0; i < Pw2.size(); i++)
		Pw2[i].resize(S.Pw[0].size());
	if(b == 19)
		cout << "???6.5" << endl;

	for(size_t i = 0; i < Pw2.size(); i++) {
		for(size_t j = 0; j < Pw2[i].size(); j++) {
			Pw2[i][j] = Pw[i][j];
		}
	}

	this->Pw = Pw2;
	// 	if(b == 19)
	// 		cout << "???7" << endl;

	// 	PRINTVAR(this->p);
	// 	PRINTVAR(p);
	// 	PRINTVAR(this->q);
	// 	PRINTVAR(q);
	
	Knotvector U2 = U;
	Knotvector V2 = V;
	U.resize((mh - ph) + this->p + 1);
	V.resize(S.Pw[0].size() + this->q + 1);
	
	if(b == 19)
		cout << "???8" << endl;

	for(size_t i = 0; i < U.size(); i++)
		U[i] = U2[i];
	for(size_t i = 0; i < V.size(); i++)
		V[i] = V2[i];

	this->n = this->Pw.size() - 1;
	this->m = this->Pw[0].size() - 1;

	// 	cout << "U: "; for(size_t i = 0; i < U.size(); i++) cout << U[i] << " "; cout << endl;
	// 	cout << "V: "; for(size_t i = 0; i < V.size(); i++) cout << V[i] << " "; cout << endl;
	// 	PRINTVAR(Pw.size());
	// 	PRINTVAR(Pw[0].size());
}

void Surface::DegreeElevateV(int t) {
	if(t <= 0){
		return;
	}
	
	Surface S = *this;
	
	int i, j, k;
	int n = S.Pw[0].size() - 1;
	int p = S.GetDegreeV();
	int m = n + p + 1;
	int ph = p + t;
	int ph2 = ph / 2;
	Matrix bezalfs(p + t + 1, p + 1) ; // coefficients for degree elevating the Bezier segment
	Array2V4n bpts(boost::extents[p + 1][S.Pw.size()]); // pth-degree Bezier control points of the current segment
	Array2V4n ebpts(boost::extents[p + t + 1][S.Pw.size()]) ; // (p+t)th-degree Bezier control points of the  current segment
	Array2V4n Nextbpts(boost::extents[p - 1][S.Pw.size()]) ; // leftmost control points of the next Bezier segment
	Vector alphas(p - 1) ; // knot instertion alphas.
	
	// Compute the Binomial coefficients
	Matrix Bin(ph + 1, ph2 + 1);
	binomialCoef(Bin);
	
	// Compute Bezier degree elevation coefficients
	nfloat inv, mpi;
	bezalfs(0, 0) = bezalfs(ph, p) = 1.0;
	for(int i = 1; i <= ph2; i++) {
		inv = 1.0 / Bin(ph, i);
		mpi = std::min(p, i);
		for(int j = std::max(0, i - t); j <= mpi; j++) {
			bezalfs(i, j) = inv * Bin(p,j) * Bin(t, i - j);
		}
	}

	for(int i = ph2 + 1; i < ph ; i++) {
		mpi = std::min(p, i);
		for(int j = std::max(0, i - t); j <= mpi ; j++)
			bezalfs(i, j) = bezalfs(ph - i, p - j);
	}
	
	// Allocate more control points than necessary
	this->Pw.resize(S.Pw.size());
	for(size_t i = 0; i < Pw.size(); i++)
		this->Pw[i].resize(S.Pw[0].size() + S.Pw[0].size() * t);
	this->p = S.GetDegreeU();
	this->q = ph;
	this->U.resize(Pw.size() + this->p + 1);
	this->V.resize(Pw[0].size() + this->q + 1);
	
	int rowJ;
	int mh = ph;
	int kind = ph + 1;
	nfloat va = S.V[0];
	nfloat vb = 0.0;
	int r = -1; 
	int oldr;
	int a = p;
	int b = p + 1; 
	int cind = 1;
	int rbz, lbz = 1;
	int mul, save, s;
	nfloat alf;
	int first, last, kj;
	nfloat den, bet, gam, numer;

	for(size_t i = 0; i < S.Pw.size(); i++)
		Pw[i][0] = S.Pw[i][0];

	for(size_t i = 0; i <= ph; i++)
		V[i] = va;
	
	// Initialize the first Bezier segment
	
	for(i = 0; i <= p; i++) {
		for(j = 0; j < S.Pw.size(); j++) {
			bpts[i][j] = S.Pw[j][i];
		}
	}
	
	while(b < m) { // Big loop thru knot vector
		i = b;

		while(b < m && S.V[b] >= S.V[b + 1]) // for some odd reasons... == doesn't work
		  	b++;
	
		mul = b - i + 1; 
		mh += mul + t;
		vb = S.V[b];
		oldr = r;
		r = p - mul;
		if(oldr > 0)
			lbz = (oldr + 2) / 2;
		else
			lbz = 1;
		if(r > 0) 
			rbz = ph - (r + 1) / 2;
		else
			rbz = ph;

		if(r > 0) { // Insert knot to get Bezier segment
			numer = vb - va ;
			for(k = p; k > mul; k--)
				alphas(k - mul - 1) = numer / (S.V[a + k] - va);

			for(j = 1;j <= r; j++) {
				save = r - j;
				s = mul + j;
				for(k = p; k>= s; k--) {
					for(rowJ = 0; rowJ < S.Pw.size(); rowJ++) {
					    bpts[k][rowJ] = alphas(k - s) * bpts[k][rowJ] + (1.0 - alphas(k - s)) * bpts[k - 1][rowJ];
					}
				}
		
				if(Nextbpts.shape()[0] > 0)
					for(rowJ = 0; rowJ < S.Pw.size(); rowJ++) {
						Nextbpts[save][rowJ] = bpts[p][rowJ];
					}
			}
		}
		
		for(i = lbz; i <= ph; i++) { // Degree elevate Bezier,  only the points lbz,...,ph are used
			for(rowJ = 0; rowJ < S.Pw.size(); rowJ++)
				ebpts[i][rowJ] = 0.0;

			mpi = std::min(p, i);
			for(j=std::max(0, i - t); j <= mpi; j++)
				for(rowJ = 0; rowJ < S.Pw.size(); rowJ++)
					ebpts[i][rowJ] += bezalfs(i,j) * bpts[j][rowJ];
		}
		
		if(oldr > 1) { // Must remove knot u=c.U[a] oldr times
			first = kind - 2;
			last = kind;
			den = vb - va;
			bet = (vb - V[kind - 1]) / den;
			for(int tr = 1; tr < oldr; tr++) { // Knot removal loop
				i = first;
				j = last;
				kj = j - kind + 1;
				while(j - i > tr) { // Loop and compute the new control points for one removal step
					if(i < cind) {
						alf = (vb - V[i]) / (va - V[i]);
						for(rowJ = 0; rowJ < S.Pw.size(); rowJ++) {
							Pw[rowJ][i] = alf * Pw[rowJ][i] + (1.0 - alf) * Pw[rowJ][i - 1];
						}
		  			}
					if(j >= lbz) {
						if(j - tr <= kind - ph + oldr) {
							gam = (vb - V[j - tr]) / den;
							for(rowJ = 0; rowJ < S.Pw.size(); rowJ++) {
								ebpts[kj][rowJ] = gam * ebpts[kj][rowJ] + (1.0 - gam) * ebpts[kj + 1][rowJ];
							}
		    			} else {
							for(rowJ = 0; rowJ < S.Pw.size(); rowJ++) {
								ebpts[kj][rowJ] = bet * ebpts[kj][rowJ] + (1.0 - bet) * ebpts[kj + 1][rowJ];
							}
						}
					}
					++i;
					--j;
					--kj;
				}
				--first;
				++last ;
			}
		}
		if(b == 19)
			cout << "???3" << endl;
		
		if(a != p) // load the knot u=c.U[a]
			for(i = 0; i < ph - oldr; i++) {
				V[kind++] = va; 
			}
		
		for(j = lbz; j <= rbz; j++) { // load control points onto nc
			for(rowJ = 0; rowJ < S.Pw.size(); rowJ++)
				Pw[rowJ][cind] = ebpts[j][rowJ]; 
			++cind ;
		}
		
		if(b < m) { // Set up for next pass thru loop
			for(rowJ = 0; rowJ < S.Pw.size(); rowJ++) {
				for(j = 0; j < r; j++)
					bpts[j][rowJ] = Nextbpts[j][rowJ];
				for(j=r;j<=p;j++)
					bpts[j][rowJ] = S.Pw[rowJ][b - p + j];
			}
			a = b; 
			b++;
			va = vb;
		} else {
			for(i = 0; i <= ph; i++)
				V[kind + i] = vb;
		}
	}
	// Resize to the proper number of control points  
	//resizeKeep(mh-ph,S.P.cols(),ph,S.degV) ;
	//
	//

	this->p = S.GetDegreeU();
	this->q = ph;
	// 	vector< vector<Vec4n> > Pw2(mh - ph);	
	vector< vector<Vec4n> > Pw2(S.Pw.size());	
	for(size_t i = 0; i < Pw2.size(); i++)
		Pw2[i].resize(mh - ph);

	for(size_t i = 0; i < Pw2.size(); i++) {
		for(size_t j = 0; j < Pw2[i].size(); j++) {
			Pw2[i][j] = Pw[i][j];
		}
	}

	this->Pw = Pw2;

	// 	PRINTVAR(this->p);
	// 	PRINTVAR(p);
	// 	PRINTVAR(this->q);
	// 	PRINTVAR(q);
	
	Knotvector U2 = U;
	Knotvector V2 = V;
	U.resize(S.Pw.size() + this->p + 1);
	V.resize((mh - ph) + this->q + 1);
	
	// 	if(b == 19)
	// 		cout << "???8" << endl;

	for(size_t i = 0; i < U.size(); i++)
		U[i] = U2[i];
	for(size_t i = 0; i < V.size(); i++)
		V[i] = V2[i];

	this->n = this->Pw.size() - 1;
	this->m = this->Pw[0].size() - 1;

	// 	cout << "U: "; for(size_t i = 0; i < U.size(); i++) cout << U[i] << " "; cout << endl;
	// 	cout << "V: "; for(size_t i = 0; i < V.size(); i++) cout << V[i] << " "; cout << endl;
	// 	PRINTVAR(Pw.size());
	// 	PRINTVAR(Pw[0].size());
}
	
std::vector<Vec4n> Surface::GetRow(int i) const {
	bool flag = true;
	size_t j = 0;
	if(i < 0) {
		j = Pw.size() + i;
	} else if(i >= 0 && i < Pw.size()) {
		j = i;
	} else {
		cerr << "error: wrong index." << endl;
		flag = false;
	}

	if(flag) {
		vector<Vec4n> pw(Pw[j].size());
		for(size_t k = 0; k < Pw[j].size(); k++) {
			const Vec4n& p = Pw[j][k];
			pw[k] = p;
		}
		return pw;
	}

	return std::vector<Vec4n>();
}

std::vector<Vec4n> Surface::GetColumn(int i) const {
	bool flag = true;
	size_t j = 0;
	if(i < 0) {
		j = Pw[0].size() + i;
	} else if(i >= 0 && i < Pw[0].size()) {
		j = i;
	} else {
		cerr << "error: wrong index." << endl;
		flag = false;
	}

	if(flag) {
		vector<Vec4n> pw(Pw.size());
		for(size_t k = 0; k < Pw.size(); k++) {
			const Vec4n& p = Pw[k][j];
			pw[k] = p;
		}
		return pw;
	}
	
	return std::vector<Vec4n>();
}

void Surface::SetRow(int i, const vector<Vec4n>& pw) {
	if(pw.size() != Pw[0].size()) {
		cerr << "error: sizes mismatch." << endl;
		return;
	}

	bool flag = true;
	size_t j = 0;
	if(i < 0) {
		j = Pw.size() + i;
	} else if(i >= 0 && i < Pw.size()) {
		j = i;
	} else {
		cerr << "error: wrong index." << endl;
		flag = false;
	}

	if(flag) {
		for(size_t k = 0; k < Pw[j].size(); k++) {
			Vec4n& p = Pw[j][k];
			p = pw[k];
		}
	}
}

void Surface::SetColumn(int i, const vector<Vec4n>& pw) {
	if(pw.size() != Pw.size()) {
		cerr << "error: sizes mismatch." << endl;
		return;
	}

	bool flag = true;
	size_t j = 0;
	if(i < 0) {
		j = Pw[0].size() + i;
	} else if(i >= 0 && i < Pw[0].size()) {
		j = i;
	} else {
		cerr << "error: wrong index." << endl;
		flag = false;
	}

	if(flag) {
		for(size_t k = 0; k < Pw.size(); k++) {
			Vec4n& p = Pw[k][j];
			p = pw[k];
		}
	}
}

void Surface::Curvature(const boost::array<nfloat, 2>& param, nfloat& k1, nfloat& k2, Vec3n& d1, Vec3n& d2) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(3);
	Derivative(param2, &SKL);
	const Vec3n& r = SKL.D[0][0];
	const Vec3n& ru = SKL.D[1][0];
	const Vec3n& rv = SKL.D[0][1];
	const Vec3n& ruu = SKL.D[2][0];
	const Vec3n& ruv = SKL.D[1][1];
	const Vec3n& rvv = SKL.D[0][2];

	Vec3n n = cross(ru, rv);
	normalize(n);

	nfloat E = dot(ru, ru);
	nfloat F = dot(ru, rv);
	nfloat G = dot(rv, rv);
	nfloat L = dot(ruu, n);
	nfloat M = dot(ruv, n);
	nfloat N = dot(rvv, n);

	nfloat a = (E * G - F * F);
	nfloat b = -(G * L + E * N - 2.0 * F * M) / a;
	nfloat c = (L * N - M * M) / a;

	// 	if(std::abs(b * b - 4.0 * c) < 0.1)
	// 		visualDebugger.AddSphere(r, math::V3n(0.0, 1.0, 0.0), 0.005);

	k1 = 0.5 * (-b - std::sqrt(b * b - 4.0 * c));
	k2 = 0.5 * (-b + std::sqrt(b * b - 4.0 * c));

	// 	if(b < 0.0)
	// 		swap(k1, k2);

	Vec2n v1, v2;
	v1[0] = (M - k1 * F) / std::sqrt((M - k1 * F) * (M - k1 * F) + (L - k1 * E) * (L - k1 * E));
	v1[1] = (L - k1 * E) / std::sqrt((M - k1 * F) * (M - k1 * F) + (L - k1 * E) * (L - k1 * E));
	v2[0] = (M - k2 * F) / std::sqrt((M - k2 * F) * (M - k2 * F) + (L - k2 * E) * (L - k2 * E));
	v2[1] = (L - k2 * E) / std::sqrt((M - k2 * F) * (M - k2 * F) + (L - k2 * E) * (L - k2 * E));

	d1 = v1[0] * ru + v1[1] * rv;
	d2 = v2[0] * ru + v2[1] * rv;
	normalize(d1);
	normalize(d2);
}

// returns d1, d2 as R2 vectors in tangent plane of surface
void Surface::Curvature(const boost::array<nfloat, 2>& param, nfloat& k1, nfloat& k2, Vec2n& d1, Vec2n& d2) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(3);
	Derivative(param2, &SKL);
	const Vec3n& r = SKL.D[0][0];
	const Vec3n& ru = SKL.D[1][0];
	const Vec3n& rv = SKL.D[0][1];
	const Vec3n& ruu = SKL.D[2][0];
	const Vec3n& ruv = SKL.D[1][1];
	const Vec3n& rvv = SKL.D[0][2];

	Vec3n n = cross(ru, rv);
	normalize(n);

	nfloat E = dot(ru, ru);
	nfloat F = dot(ru, rv);
	nfloat G = dot(rv, rv);
	nfloat L = dot(ruu, n);
	nfloat M = dot(ruv, n);
	nfloat N = dot(rvv, n);

	nfloat a = (E * G - F * F);
	nfloat b = -(G * L + E * N - 2.0 * F * M) / a;
	nfloat c = (L * N - M * M) / a;

	// 	if(std::abs(b * b - 4.0 * c) < 0.1)
	// 		visualDebugger.AddSphere(r, math::V3n(0.0, 1.0, 0.0), 0.005);

	k1 = 0.5 * (-b - std::sqrt(b * b - 4.0 * c));
	k2 = 0.5 * (-b + std::sqrt(b * b - 4.0 * c));

	// 	if(b < 0.0)
	// 		swap(k1, k2);

	d1[0] = (M - k1 * F) / std::sqrt((M - k1 * F) * (M - k1 * F) + (L - k1 * E) * (L - k1 * E));
	d1[1] = (L - k1 * E) / std::sqrt((M - k1 * F) * (M - k1 * F) + (L - k1 * E) * (L - k1 * E));
	d2[0] = (M - k2 * F) / std::sqrt((M - k2 * F) * (M - k2 * F) + (L - k2 * E) * (L - k2 * E));
	d2[1] = (L - k2 * E) / std::sqrt((M - k2 * F) * (M - k2 * F) + (L - k2 * E) * (L - k2 * E));
	
	normalize(d1);
	normalize(d2);

}

void Surface::CurvatureFirstDerivative(const boost::array<nfloat, 2>& param, const bool& is_k1, nfloat& ku, nfloat& kv) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(4);
	Derivative(param2, &SKL);
	const Vec3n& r = SKL.D[0][0];
	const Vec3n& ru = SKL.D[1][0];
	const Vec3n& rv = SKL.D[0][1];
	const Vec3n& ruu = SKL.D[2][0];
	const Vec3n& ruv = SKL.D[1][1];
	const Vec3n& rvv = SKL.D[0][2];
	const Vec3n& ruuu = SKL.D[3][0];
	const Vec3n& ruuv = SKL.D[2][1];
	const Vec3n& ruvv = SKL.D[1][2];
	const Vec3n& rvvv = SKL.D[0][3];

	Vec3n n = cross(ru, rv);
	// 	normalize(n);

	nfloat E = dot(ru, ru);
	nfloat F = dot(ru, rv);
	nfloat G = dot(rv, rv);
	nfloat L = dot(ruu, n);
	nfloat M = dot(ruv, n);
	nfloat N = dot(rvv, n);

	nfloat Eu, Ev, Fu, Fv, Gu, Gv,
		Lu, Lv, Mu, Mv, Nu, Nv;

	Eu = 2.0 * dot(ruu,ru); Ev = 2.0 * dot(ruv,ru);
	Fu = dot(ruu,rv) + dot(ru, ruv); Fv = dot(ruv,rv) + dot(ru, rvv);
	Gu = 2.0 * dot(ruv,rv); Gv = 2.0 * dot(rvv,rv);

	Vec3n ru_x_rv = cross(ru, rv),
		ru_x_ruv = cross(ru, ruv),
		ruv_x_rv = cross(ruv, rv),
		ru_x_rvv = cross(ru, rvv),
		ruu_x_rv = cross(ruu, rv);

	Lu = dot(ruuu, ru_x_rv) + dot(ruu,ru_x_ruv);
	Lv = dot(ruuv, ru_x_rv) + dot(ruu, ruv_x_rv) + dot(ruu, ru_x_rvv);;

	Mu = dot(ruuv, ru_x_rv) + dot(ruv, ruu_x_rv);
	Mv = dot(ruvv, ru_x_rv) + dot(ruv, ru_x_rvv);

	Nu = dot(ruvv, ru_x_rv) + dot(rvv, ruu_x_rv) + dot(rvv, ru_x_ruv);
	Nv = dot(rvvv, ru_x_rv) + dot(rvv, ruv_x_rv);

	nfloat a = (E * G - F * F);
	nfloat b = -(G * L + E * N - 2.0 * F * M);
	nfloat c = (L * N - M * M);

	nfloat au, av, bu, bv, cu, cv;

	au = Eu*G + E*Gu - 2.0*F*Fu;
	av = Ev*G + E*Gv - 2.0*F*Fv;

	bu = 2.0*F*Mu + 2.0*Fu*M - G*Lu - Gu*L - E*Nu - Eu*N;
	bv = 2.0*F*Mv + 2.0*Fv*M - G*Lv - Gv*L - E*Nv - Ev*N;

	cu = L*Nu + Lu*N - 2.0*M*Mu;
	cv = L*Nv + Lv*N - 2.0*M*Mv;

	nfloat d = std::sqrt(b * b - 4.0 * a * c);

	//ku
	nfloat ku1 = -pow(a,-1.5)*bu + 1.5*pow(a,-2.5)*au*b;
	nfloat ku2 = pow(a,-1.5)*b*bu - 2.0*pow(a,-0.5)*cu + 4.0*pow(a,-1.5)*au*c - 1.5*pow(a,-2.5)*au*pow(b,2.0);
	
	if(is_k1)
		ku = 0.5*(ku1 + ku2/d);
	else
		ku = 0.5*(ku1 - ku2/d);

	//kv
	nfloat kv1 = -pow(a,-1.5)*bv + 1.5*pow(a,-2.5)*av*b;
	nfloat kv2 = pow(a,-1.5)*b*bv - 2.0*pow(a,-0.5)*cv + 4.0*pow(a,-1.5)*av*c - 1.5*pow(a,-2.5)*av*pow(b,2.0);
	
	if(is_k1)
		kv = 0.5*(kv1 + kv2/d);
	else
		kv = 0.5*(kv1 - kv2/d);
}

void Surface::CurvatureSecondDerivative(const boost::array<nfloat, 2>& param, const bool& is_k1, nfloat& kuu, nfloat& kuv, nfloat& kvv) const {
	cout << "Not yet implemented!" << endl;
}

void Surface::Curvature(const boost::array<nfloat, 2>& param, boost::array<nfloat, 6>& C, nfloat& k1, nfloat& k2) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(2);
	Derivative(param2, &SKL);
	const Vec3n& r = SKL.D[0][0];
	const Vec3n& ru = SKL.D[1][0];
	const Vec3n& rv = SKL.D[0][1];
	const Vec3n& ruu = SKL.D[2][0];
	const Vec3n& ruv = SKL.D[1][1];
	const Vec3n& rvv = SKL.D[0][2];

	Vec3n n = cross(ru, rv);
	normalize(n);

	nfloat E = dot(ru, ru);
	nfloat F = dot(ru, rv);
	nfloat G = dot(rv, rv);
	nfloat L = dot(ruu, n);
	nfloat M = dot(ruv, n);
	nfloat N = dot(rvv, n);
	
	nfloat a = (E * G - F * F);
	nfloat b = (G * L + E * N - 2.0 * F * M) / a;
	nfloat c = (L * N - M * M) / a;

	k1 = 0.5 * (-b - std::sqrt(b * b - 4.0 * c));
	k2 = 0.5 * (-b + std::sqrt(b * b - 4.0 * c));

	C[0] = E;
	C[1] = F;
	C[2] = G;
	C[3] = L;
	C[4] = M;
	C[5] = N;
}
	
Vec3n Surface::Normal(const boost::array<nfloat, 2>& param) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(1);
	Derivative(param2, &SKL);
	const Vec3n& ru = SKL.D[1][0];
	const Vec3n& rv = SKL.D[0][1];

	Vec3n n = cross(ru, rv);
	normalize(n);

	return n;
}

Vec3n Surface::Normal(nfloat u, nfloat v) const {
	boost::array<nfloat, 2> param = {u, v};
	return Normal(param);
}

std::pair<Vec3n, Vec3n> Surface::Partials(const boost::array<nfloat, 2>& param) const {
	nfloat param2[2] = {param[0], param[1]};
	DerivativesDim2 SKL(1);
	Derivative(param2, &SKL);
	const Vec3n& ru = SKL.D[1][0];
	const Vec3n& rv = SKL.D[0][1];

	return std::pair<Vec3n, Vec3n>(ru, rv);
}

std::pair<Vec3n, Vec3n> Surface::Partials(const nfloat u, const nfloat v) const {
	boost::array<nfloat, 2> param = {u, v};
	return Partials(param);
}

nfloat nu(const boost::array<nfloat, 6>& C, nfloat k) {
	const nfloat& E = C[0];
	const nfloat& F = C[1];
	const nfloat& G = C[2];
	const nfloat& L = C[3];
	const nfloat& M = C[4];
	const nfloat& N = C[5];

	nfloat t1 = E * (M + k * F) * (M + k * F);
	nfloat t2 = 2.0 * F * (M + k * F) * (L + k * E);
	nfloat t3 = G * (L + k * E) * (L + k * E);

	return 1.0 / (std::sqrt(t1 - t2 + t3));
}

nfloat mu(const boost::array<nfloat, 6>& C, nfloat k) {
	const nfloat& E = C[0];
	const nfloat& F = C[1];
	const nfloat& G = C[2];
	const nfloat& L = C[3];
	const nfloat& M = C[4];
	const nfloat& N = C[5];

	nfloat t1 = E * (N + k * G) * (N + k * G);
	nfloat t2 = 2.0 * F * (N + k * G) * (M + k * F);
	nfloat t3 = G * (M + k * F) * (M + k * F);

	return 1.0 / std::sqrt(t1 - t2 + t3);
}

bool Surface::fD(Vec2n& c, const Vec2n& u, nfloat h, nfloat sign) const {
	if(u[0] < 0.0 || u[0] > 1.0 || u[1] < 0.0 || u[1] > 1.0) 
		return false;

	boost::array<nfloat, 6> C;
	nfloat& E = C[0];
	nfloat& F = C[1];
	nfloat& G = C[2];
	nfloat& L = C[3];
	nfloat& M = C[4];
	nfloat& N = C[5];
	nfloat k1, k2;
	Vec3n d1, d2;

	boost::array<nfloat, 2> u0 = {u[0], u[1]};
	Curvature(u0, C, k1, k2);

	nfloat k = k1;

	nfloat fu, fv;
	if(std::abs(L + k * E) >= std::abs(N + k * G)) {
		fu = nu(C, k) * (M + k * F);
		fv = -nu(C, k) * (L + k * E);
	} else {
		fu = mu(C, k) * (N + k * G);
		fv = -mu(C, k) * (M + k * F);
	}
	
	c = (sign * h) * math::V2n(fu, fv);

	return true;
}

vector<Vec2n> Surface::Trace(const Vec2n& u0, nfloat h) const {
	Vec2n uk = math::V2n(u0[0], u0[1]);
	Vec2n dup, du;

	vector<Vec2n> param(1);
	param[0] = uk;
	nfloat sign = 1.0;
	DerivativesDim2 SKL(1);

	Vec3n rup, rvp, ru, rv;
	size_t i = 0;
	while(true) {
		if(i > 100000)
			return param;

		dup = du;
		rup = ru;
		rvp = rv;

		Vec2n c1, c2, c3, c4;
		if(!fD(c1, uk, h, sign))
			return param;

		if(!fD(c2, uk + 0.5 * c1, h, sign))
			return param;

		if(!fD(c3, uk + 0.5 * c2, h, sign))
			return param;

		if(!fD(c4, uk + c3, h, sign))
			return param;
		
		du = (1.0 / 6.0) * (c1 + 2.0 * c2 + 2.0 * c3 + c4);
		uk = uk + du;
		// 		cout << i << ": " << uk << " " << du << endl;

		nfloat uv[2] = {uk[0], uk[1]};
		Derivative(uv, &SKL);
		ru = SKL.D[1][0];
		rv = SKL.D[0][1];
		cout << du << endl;

		if(i > 0) {
			Vec3n a = dup[0] * rup + dup[1] * rvp;
			Vec3n b = du[0] * ru + du[1] * rv;
			if(length(a + b) < length(a - b)) {
				sign *= -1.0;
			}
		}
		
		i++;
		param.push_back(uk);
	}

	return param;
}

void ConvertBasis(vector< vector< Vec4n > >& P, size_t degree) {
	Matrix B(4, 4);
	B(0, 0) = 1.0;
	B(0, 1) = 1.0;
	B(0, 2) = 1.0;
	B(0, 3) = 1.0;

	B(1, 0) = -1.0;
	B(1, 1) = -1.0 / 3.0;
	B(1, 2) = 1.0 / 3.0;
	B(1, 3) = 1.0;

	B(2, 0) = 1.0;
	B(2, 1) = -1.0 / 3.0;
	B(2, 2) = -1.0 / 3.0;
	B(2, 3) = 1.0;

	B(3, 0) = -1.0;
	B(3, 1) = 1.0;
	B(3, 2) = -1.0;
	B(3, 3) = 1.0;

	Matrix Bt = trans(B);

	for(size_t k = 0; k < 3; k++) {
		Matrix C1(4, 4);
		for(size_t i = 0; i < 4; i++)
			for(size_t j = 0; j < 4; j++)
				C1(i, j) = P[i][j][k];

		Matrix C2 = prod(Bt, C1);
		C2 = prod(C2, B);

		for(size_t i = 0; i < 4; i++)
			for(size_t j = 0; j < 4; j++)
				P[i][j][k] = C2(i, j);
	}
}

Vec3n EvaluatePowerBasis(const boost::array<nfloat, 2>& param, const vector< vector< Vec4n> >& P, size_t degree) {
	const nfloat& u = param[0];
	const nfloat& v = param[1];
	Vec4n c = math::V4n(0.0, 0.0, 0.0, 0.0);
	for(size_t i = 0; i <= degree; i++)
		for(size_t j = 0; j <= degree; j++)
			c += P[i][j] * math::power::BasisFun(u, i) * math::power::BasisFun(v, j);
	return math::V3n(c[0], c[1], c[2]);
}

Surface Surface::WeightedLeastSquares(const std::vector< std::pair<Vec2n, Vec3n> >& p, size_t degree, const std::vector<nfloat>& thetad, bool usePowerBasis) {
	if(p.size() != thetad.size()) {
		cout << "error WeightedLeastSquares: p and thetad need to be of the same size." << endl;
	}
	if(usePowerBasis)
		cout << "Applying LS using power basis..." << endl;
	
	size_t degree2 = (degree + 1) * (degree + 1);
	Matrix B(p.size(), degree2);
	Knotvector U = math::spline::GenerateOpenKnotvector<nfloat>(degree, degree + 1);
	
	using namespace math::spline;
	for(size_t i = 0; i < B.size1(); i++) {
		const Vec2n& param = p[i].first;
		nfloat u = param[0];
		nfloat v = param[1];
		
		for(size_t j = 0; j <= degree; j++) {
			for(size_t k = 0; k <= degree; k++) {
				if(usePowerBasis)
					B(i, j * (degree + 1) + k) = math::power::BasisFun(u, j) * math::power::BasisFun(v, k);
				else
					B(i, j * (degree + 1) + k) = math::spline::BasisFun(u, j, degree, U) * math::spline::BasisFun(v, k, degree, U);
			}
		}
	}
	
	Matrix A = ZeroMatrix(degree2, degree2);
	
	for(size_t j = 0; j < degree2; j++)
		for(size_t k = 0; k < degree2; k++)
			for(size_t i = 0; i < p.size(); i++)
				A(j, k) += thetad[i] * B(i, j) * B(i, k);
	
	Vector Cx = ZeroVector(degree2);
	Vector Cy = ZeroVector(degree2);
	Vector Cz = ZeroVector(degree2);
	for(size_t j = 0; j < degree2; j++)
		for(size_t i = 0; i < p.size(); i++) {
			const Vec3n& P = p[i].second;
			Cx(j) += thetad[i] * B(i, j) * P[0];
			Cy(j) += thetad[i] * B(i, j) * P[1];
			Cz(j) += thetad[i] * B(i, j) * P[2];
		}

	Vector Xx(degree2), Xy(degree2), Xz(degree2);
	if(!math::Solve(A, Cx, Xx) || !math::Solve(A, Cy, Xy) || !math::Solve(A, Cz, Xz))
		cerr << "Surface interpolation: could not solve linear system in x." << endl;
	
	if(norm_2(prod(A, Xx) - Cx) > 0.01 || norm_2(prod(A, Xy) - Cy) > 0.01 || norm_2(prod(A, Xz) - Cz) > 0.01)
		cout << "Error: Least Squares produces large error." << endl;
	
	vector< vector<Vec4n> > Pw(degree + 1);
	for(size_t i = 0; i < degree + 1; i++)
		Pw[i] = vector<Vec4n>(degree + 1);
	
	for(size_t i = 0; i <= degree; i++) {
		for(size_t j = 0; j <= degree; j++) {
			size_t k = i * (degree + 1) + j;
			Pw[i][j] = math::V4n(Xx(k), Xy(k), Xz(k), 1.0);
		}
	}

	vector< vector<Vec4n> > Pw2 = Pw;
	if(usePowerBasis)
		ConvertBasis(Pw2, degree);
		
	Surface surface2(Pw2, degree, degree, U, U);

	if(usePowerBasis) {
		for(size_t i = 0; i < 10; i++) {
			for(size_t j = 0; j < 10; j++) {
				boost::array<nfloat, 2> param2 = {(nfloat)i / 9, (nfloat)j / 9};
				boost::array<nfloat, 2> param = {2.0 * param2[0] - 1.0, 2.0 * param2[1] - 1.0};
				Vec3n Ptemp2 = surface2.Evaluate(param2);
				Vec3n Ptemp = EvaluatePowerBasis(param, Pw, degree);
				if(length(Ptemp2 - Ptemp) > 0.001) {
					cout << "error: problem with changing basis: " << Ptemp2 << " vs. " << Ptemp << endl;
				}
			}
		}
	}

	return surface2;
}

Surface Surface::LeastSquares(const std::vector< std::pair<Vec2n, Vec3n> >& p, size_t degree, bool usePowerBasis) {
	vector<nfloat> thetad(p.size());
	for(size_t i = 0; i < thetad.size(); i++)
		thetad[i] = 1.0;
	return Surface::WeightedLeastSquares(p, degree, thetad, usePowerBasis);
}

void Surface::Transpose() {
	size_t n = Pw.size();
	size_t m = Pw[0].size();

 	vector< vector<Vec4n> > Pw2(m);
	for(size_t i = 0; i < m; i++) {
		Pw2[i] = vector<Vec4n>(n);
		for(size_t j = 0; j < n; j++) {
			Pw2[i][j] = Pw[j][i];
		}
	}
	Pw = Pw2;
	std::swap(U, V);
	std::swap(p, q);
	std::swap(this->n, this->m);
}

void Surface::FlipU() {
	reverse(Pw.begin(), Pw.end());
	reverse(U.begin(), U.end());
	for(size_t i = 0; i < U.size(); i++)
		U[i] = 1.0 - U[i];
}

void Surface::FlipV() {
	for(size_t i = 0; i < Pw.size(); i++) {
		reverse(Pw[i].begin(), Pw[i].end());
	}
	reverse(V.begin(), V.end());
	for(size_t i = 0; i < V.size(); i++)
		V[i] = 1.0 - V[i];		
}
